/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "COccuMCL.h"
#include "CTopoScanOccuMCL.h"
#include "CTopoOccuMCL.h"
#include "EnvironmentMeasurement2D.h"

/// For reading the data from file
int readScanLine181(FILE *f,ScanMatch::scan &meas, ScanMatch::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

void scan2absoluteXY( ScanMatch::scan &s, float *x, float *y, ScanMatch::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			setRGBpix(rgb,i,j, Nx, r*data[Nx*j+i],g*data[Nx*j+i],b*data[Nx*j+i]);
		}
	}	
}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}


int filterScan(ScanMatch::scan &m, Grid::TScan &g){
		int bad=0;
		float dist_th=1.0;
		float dr_plus,dr_minus;
		
		for(int i=1;i<m.N-1;i++){
				dr_plus = fabs(m.r[i]-m.r[i+1]);
				dr_minus = fabs(m.r[i]-m.r[i-1]);
				if(dr_plus>dist_th && dr_minus>dist_th) g.r[i] = -1.0; 
		}
		for(int i=0;i<m.N;i++){
				g.a[i] = m.a[i];
				if(g.r[i] == -1){ g.r[i]=0; bad++;} 
				else g.r[i] = m.r[i];
				if(g.r[i] > 35){g.r[i] = 0; bad++;}///Filter out long distances
		}
		return bad;
}

int main(void){
		float grid_size = 45.0; ///< size of the local grid
		
		TOccupancyGrid O(grid_size,grid_size,0.04,Grid::TCoord2D(0,0));
		TOccupancyGrid O_10(grid_size,grid_size,0.1,Grid::TCoord2D(0,0));
		TOccupancyGrid O_15(grid_size,grid_size,0.15,Grid::TCoord2D(0,0));
		O.setGridToValue(0.5);
		O_10.setGridToValue(0.5);
		O_15.setGridToValue(0.5);
		CEnvironmentMeasurement2D env_meas;
		
		//COccuMCL omcl;
		CTopoOccuMCL omcl;
		
		grid_t *origmap;
		grid_t *dataPtr;
		int Nx,Ny;
		int oNx,oNy;
		unsigned char *rgb;
		
		FILE *fenv_meas = fopen("topo_environment_measurement2D.txt","wt");
		if(fenv_meas==NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		FILE *fenv_meas_10 = fopen("topo_environment_measurement2D_10cm.txt","wt");
		if(fenv_meas==NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		FILE *fenv_meas_15 = fopen("topo_environment_measurement2D_15cm.txt","wt");
		if(fenv_meas==NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		
		FILE *log = fopen("../../../data/j2b2_measurement.txt","rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		/*
		omcl.initialize("../../res/autmap-new.txt",
										1.0, 
										3000, 
										mcl::pose(10,-5,-10*M_PI/180), 
										mcl::pose(1.,1.,20.0*M_PI/180.0),
										10,
										-M_PI/2.0,
										0.1,
										1.0);
		*/
		omcl.initialize("../../res/autmap-new.txt",
										 1.0, 
										 3000, 
										 mcl::pose(10,-5,-10*M_PI/180), 
										 mcl::pose(1.,1.,20.0*M_PI/180.0),
										 0.1,
										 1.0);
		
		
		
		omcl.O->getDim(oNx,oNy);
		fprintf(stderr,"Map size = %dx%d\n",oNx,oNy);
		origmap = (grid_t *) malloc(oNx*oNy*sizeof(float));
		dataPtr = omcl.O->getDataPtr();
		memcpy(origmap,dataPtr,oNx*oNy*sizeof(float));		
				
		mcl::pose mpos,mest;
		mcl::scan mvirt;
		mvirt.allocate(181);
		
		
		///Scanmatch stuff
		CScanMatchInterface *gmatcher; ///< This is for global	
		gmatcher = new CAHistogramECorrelationScanMatch(
																						.6,   // Search area in meters 
																						0.6);     // Search angle in radians
		
		ScanMatch::pose pos,odo_cur,odo_ref,est;   ///< The estimated position
		ScanMatch::scan meas,ref,meas0;  ///< The Reference scan 
		meas.allocate(181);
		ref.allocate(181);
		meas0.allocate(181);		
		///Initialize grid as empty
 		
		O.getDim(Nx,Ny);
		
		///Initialize Image display
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		unsigned char *orgb = (unsigned char *) malloc(3*oNx*oNy);
		addImageWindow(Nx, Ny);
		addImageWindow(oNx, oNy);
		glutImageGUI_Start();
		
		Grid::TScan s,virt; ///< for occupancy grid
		s.alloc(181);
		virt.alloc(181);
		readScanLine181(log,meas0,odo_cur);
	
		odo_cur.set(0,0,0);
		O.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, *((Grid::TScan *) &meas0),0.01,0.99);
		O.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		odo_ref = odo_cur;
    fprintf(stderr,"ref.N=%d meas.N=%d  \n",ref.N,meas.N);
		
		float updatesincedist=1000;
		mcl::pose g_origin;
		mcl::pose g_global_origin;
		vector<mcl::pose> orig;
		vector<mcl::pose> orig_est;
		vector<float> weights;
		bool updateOrigin = true;
		bool origin_updated = false;
		
		mcl::pose mp_origin;
		mcl::pose mv_origin;
		mcl::pose mcl_odo_ref;
		mcl::pose mp;
		mcl_odo_ref.set(odo_cur.x,odo_cur.y,odo_cur.a);
		
		while(!feof(log)){
				readScanLine181(log,meas,odo_cur);
				meas.filt(25.0);
				////////////////////////////////////////////////////////////////////////////
				/// The SLAM
				////////////////////////////////////////////////////////////////////////////
				
				est = est.calculateDifferentialSensorMovement(odo_ref,odo_cur, 0);
				odo_ref = odo_cur;
				pos = pos.integrateDifferential(est);
				O.getScan(pos.x,pos.y, (pos.a-M_PI/2.0), 181, M_PI/180.0, (grid_t) 0.7, virt); ///<getscan
				for(int i=0;i<meas.N;i++){
						ref.a[i]=virt.a[i]-M_PI/2.0;
						ref.r[i]=virt.r[i]; 
						if (virt.r[i]<0)ref.r[i]=0;
				}	
				gmatcher->setCurrentPose(pos);
				gmatcher->setReferenceScan(ref,pos); 
				gmatcher->update(meas,pos);    ///update
				gmatcher->getCurrentPose(pos); ///< Get the estimate			
				
				///Set measurement to map
				filterScan(meas,s);
				O.setScanFillProb(pos.x, pos.y ,pos.a,s,0.3,0.9);
				O_10.setScanFillProb(pos.x, pos.y ,pos.a,s,0.1,0.9);
				O_15.setScanFillProb(pos.x, pos.y ,pos.a,s,0.1,0.9);
				
				////////////////////////////////////////////////////////////////////////////
				/// The MCL
				////////////////////////////////////////////////////////////////////////////
				//mest.setToDifferentialPose(  *((mcl::pose *) &odo_cur),*((mcl::pose *) &odo_ref));
				if(cnt%5==0){
					mest.setToDifferentialPose(  *((mcl::pose *) &odo_cur),mcl_odo_ref);
					mcl_odo_ref.set(odo_cur.x,odo_cur.y,odo_cur.a);
					mest.toPI();
					//fprintf(stderr,"%.2f, %.2f,%.2f\n",mest.x,mest.y,mest.a);
					omcl.runMCL(mest, mcl::pose(fabs(mest.x)*0.3,fabs(mest.y)*0.3,0.1*fabs(mest.a)),  
											*((mcl::scan *) &meas));
					
					mp = omcl.filt.getDistributionMean(true); ///< The distribution mean
					mcl::pose mcv = omcl.filt.getDistributionVariances(); ///< The distribution variance
					
					fprintf(stderr,"Est:(%.2f, %.2f,%.2f) Mp:(%.2f, %.2f,%.2f) Mp:(%.2f, %.2f,%.2f)\n",
									mest.x,mest.y,mest.a,mp.x,mp.y,mp.a,2*sqrt(mcv.x),2*sqrt(mcv.y),sqrt(fabs(mcv.a))*180.0/M_PI);
					///Save the initial estimate of the origin
					if(!origin_updated){
							origin_updated = true;
							mp_origin = omcl.filt.getDistributionMean(true);
							mv_origin = omcl.filt.getDistributionVariances(); ///< The distribution mean
					}
					mp.to2PI();
				}
				////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
				////////////////////////////////////////////////////////////////////////////
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				if(toEdge < 10.0 || feof(log)){
						env_meas.setOccupancyGrid(&O, 0.8); ///< set grid to 2D measurement 
						env_meas.frame.setFrame(mp_origin.x,mp_origin.y,mp_origin.a);		
						env_meas.frame.setError(mv_origin.x,mv_origin.y,mv_origin.a);		
						
						fprintf(stderr,"Saving Original map\n");
						env_meas.fprint(fenv_meas);        ///< Save to file
						fflush(fenv_meas);                 ///< flush
						
						fprintf(stderr,"Saving map with 10cm resolution\n");
						env_meas.setOccupancyGrid(&O_10, 0.8); ///< set grid to 2D measurement 
						env_meas.fprint(fenv_meas_10);        ///< Save to file
						fflush(fenv_meas_10);                 ///< flush
						
						fprintf(stderr,"Saving map with 15cm resolution\n");
						env_meas.setOccupancyGrid(&O_15, 0.8); ///< set grid to 2D measurement 
						env_meas.fprint(fenv_meas_15);        ///< Save to file
						fflush(fenv_meas_15);                 ///< flush
						
						O.setGridToValue(0.5); ///< Clear map
						O_10.setGridToValue(0.5); ///< Clear map
						O_15.setGridToValue(0.5); ///< Clear map
						
						pos.set(0,0,0); ///< initialize pose
						O.setScanFillProb(0, 0 ,0, *((Grid::TScan *) &meas),0.01,0.99); ///< set last measurement to map
						O_10.setScanFillProb(0, 0 ,0, *((Grid::TScan *) &meas),0.01,0.99); ///< set last measurement to map
						O_15.setScanFillProb(0, 0 ,0, *((Grid::TScan *) &meas),0.01,0.99); ///< set last measurement to map
						origin_updated = false;
				}
				
				////////////////////////////////////////////////////////////////////////////
				/// Visualisation
				////////////////////////////////////////////////////////////////////////////
				/// SLAM

				if(cnt%100==0){
					//fprintf(stdout,"Drawing");
					O.fillRGB(rgb,1.0/255.0);
					displayImage(0, rgb,Ny,Nx);
					
					/// MCL
					
					setRGBmap(orgb,omcl.O, 0,255, 0);
					Grid::TCoord2D coord;
					Grid::TPixel pix;
					for(int i=0;i<omcl.filt.NumOfParticles;i++){
						coord.x = omcl.filt.Particles[i].x;
						coord.y = omcl.filt.Particles[i].y;
						pix = omcl.O->toPixel(coord);
						setRGBpix(orgb,pix.x,pix.y, oNx, 255,255, 0);
					}
					
					omcl.getScan(mvirt, mp);
					//omcl.getScan(mvirt, mp);
					for(int i=0;i<mvirt.N;i++){
						coord.x = mp.x + mvirt.r[i]*cos(mvirt.a[i]+mp.a);
						coord.y = mp.y + mvirt.r[i]*sin(mvirt.a[i]+mp.a);
						pix = omcl.O->toPixel(coord);
						if(pix.x>=0 && pix.x<oNx&&pix.y>=0&&pix.y<oNy)
							setRGBpix(orgb,pix.x,pix.y, oNx, 255,0, 0);
					}

					for(int i=0;i<meas.N;i++){
						coord.x = mp.x + meas.r[i]*cos(meas.a[i]+mp.a);
						coord.y = mp.y + meas.r[i]*sin(meas.a[i]+mp.a);
						pix = omcl.O->toPixel(coord);
						if(pix.x>=0 && pix.x<oNx&&pix.y>=0&&pix.y<oNy)
							setRGBpix(orgb,pix.x,pix.y, oNx, 0,255, 255);
					}
					Grid::TCoord2D ocoord;
					ocoord.x = g_origin.x;
					ocoord.y = g_origin.y;
					Grid::TPixel opix = omcl.O->toPixel(ocoord);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,g_origin.a, 2.0,255,0,0);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,g_origin.a+M_PI/2, 2.00,0,255,0);
					ocoord.x = mp.x;ocoord.y = mp.y;
					opix = omcl.O->toPixel(ocoord);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,mp.a, 2.0,255,0,0);
					drawLine(orgb, oNx,oNy, omcl.O->getResolution(),opix.x,opix.y,mp.a+M_PI/2, 2.0,0,255,0);
				
					displayImage(1, orgb,oNy,oNx);
					
				}
				////////////////////////////////////////////////////////////////////////////
				/// The end
				////////////////////////////////////////////////////////////////////////////
				
				cnt++;
		}
		fclose(fenv_meas);
		fclose(fenv_meas_10);
		fclose(fenv_meas_15);
		
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
		
		fprintf(stderr,"The End - press ctrl C to stop\n");
		while(1) usleep(1000*1000);
		
		return 0;
}